#include "stdafx.h"
#include "Matrix3.h"

Matrix3::Matrix3()
{
	for(int i=0;i<3;i++)
	{
		for(int j=0;j<3;j++)
		{
			mt[i][j]=0;
		}
	}
}

Matrix3::Matrix3( float m00, float m01, float m02, float m10, float m11, float m12, float m20, float m21, float m22 )
{  
	setMatrix3(m00,m01,m02,m10,m11,m12,m20,m21,m22);
}

Matrix3::Matrix3( const Vector3 &v )
{
	mt[0][0]=0;		mt[0][1]=-v.z;  mt[0][2]=v.y;
	mt[1][0]=v.z;	mt[1][1]=0;		mt[1][2]=-v.x;
	mt[2][0]=-v.y;	mt[2][1]=v.x;	mt[2][2]=0;
}

void Matrix3::setMatrix3( float m00, float m01, float m02, float m10, float m11, float m12, float m20, float m21, float m22 )
{  
	mt[0][0]= m00, mt[0][1]= m01, mt[0][2]= m02;
	mt[1][0]= m10, mt[1][1]= m11, mt[1][2]= m12;
	mt[2][0]= m20, mt[2][1]= m21, mt[2][2]= m22;
}

Matrix3 & Matrix3::operator=( const Matrix3 &mat )
{
	for(int i=0;i<3;i++)
	{
		for(int j=0;j<3;j++)
		{
			mt[i][j]=mat.mt[i][j];
		}
	}
	return *this;
}

void Matrix3::rotate_X( Matrix3 &m, const float angle )
{
	m.mt[0][0]= 1; m.mt[0][1]= 0;			m.mt[0][2]= 0;
	m.mt[1][0]= 0; m.mt[1][1]= cosf(angle); m.mt[1][2]= sinf(angle);
	m.mt[2][0]= 0; m.mt[2][1]=-sinf(angle); m.mt[2][2]= cosf(angle);
}

void Matrix3::rotate_Y( Matrix3 &m, const float angle )
{
	m.mt[0][0]= cosf(angle);	m.mt[0][1]= 0; m.mt[0][2]= -sinf(angle);
	m.mt[1][0]= 0;				m.mt[1][1]= 0; m.mt[1][2]= 0;
	m.mt[2][0]= sinf(angle);	m.mt[2][1]= 0; m.mt[2][2]= cosf(angle);
}

void Matrix3::rotate_Z( Matrix3 &m, float angle )
{
	m.mt[0][0]= cosf(angle);	m.mt[0][1]= sinf(angle);  m.mt[0][2]= 0;
	m.mt[1][0]= -sinf(angle);	m.mt[1][1]= cosf(angle);  m.mt[1][2]= 0;
	m.mt[2][0]= 0;				m.mt[2][1]= 0;            m.mt[2][2]= 1;
}

void Matrix3::rotate( Matrix3 &m, const Vector3 &v, const float angle )
{
	float c= 1-cosf(angle);
	float s= sinf(angle);
	float cxy= c*v.x*v.y;
	float cxz= c*v.x*v.z;
	float cyz= c*v.y*v.z;

	m.mt[0][0]= c*v.x*v.x + 1-c;	m.mt[0][1]= cxy     - v.z*s;	m.mt[0][2]= cxz + v.y*s; 
	m.mt[1][0]= cxy     + v.z*s;	m.mt[1][1]= c*v.y*v.y + 1-c;	m.mt[1][2]= cyz - v.x*s; 
	m.mt[2][0]= cxz     - v.y*s;	m.mt[2][1]= cyz     + v.x*s;	m.mt[2][2]= c*v.z*v.z + 1-c; 
}

void Matrix3::rotate_Euler( Matrix3 &m, float x, float y, float z )
{
	double cx = cos(x);
	double sx = sin(x);
	double cy = cos(y);
	double sy = sin(y);
	double cz = cos(z);
	double sz = sin(z);

	m.mt[0][0] = (float)(cy*cz); 
	m.mt[0][1] = (float)(cy*sz);
	m.mt[0][2] = (float)(-sy);

	double sxsy = sx*sy;
	double cxsy = cx*sy;

	m.mt[1][0] = (float)(sxsy*cz-cx*sz);
	m.mt[1][1] = (float)(sxsy*sz+cx*cz);
	m.mt[1][2] = (float)(sx*cy);

	m.mt[2][0] = (float)(cxsy*cz+sx*sz);
	m.mt[2][1] = (float)(cxsy*sz-sx*cz);
	m.mt[2][2] = (float)(cx*cy);
}

void Matrix3::rotate_Euler( Matrix3 &m, const Vector3 &angles )
{	
	rotate_Euler(m, angles.x, angles.y, angles.z);	
}

Matrix3 & Matrix3::operator*=( const Matrix3 &m )
{
	Matrix3 temp(
		mt[0][0]*m.mt[0][0] + mt[1][0]*m.mt[0][1] + mt[2][0]*m.mt[0][1],
		mt[0][1]*m.mt[0][0] + mt[1][1]*m.mt[0][1] + mt[2][1]*m.mt[0][2],
		mt[0][2]*m.mt[0][0] + mt[1][2]*m.mt[0][1] + mt[2][2]*m.mt[0][2],

		mt[0][0]*m.mt[1][0] + mt[1][0]*m.mt[1][1] + mt[2][0]*m.mt[1][1],
		mt[0][1]*m.mt[1][0] + mt[1][1]*m.mt[1][1] + mt[2][1]*m.mt[1][2],
		mt[0][2]*m.mt[1][0] + mt[1][2]*m.mt[1][1] + mt[2][2]*m.mt[1][2],

		mt[0][0]*m.mt[2][0] + mt[1][0]*m.mt[2][1] + mt[2][0]*m.mt[2][1],
		mt[0][1]*m.mt[2][0] + mt[1][1]*m.mt[2][1] + mt[2][1]*m.mt[2][2],
		mt[0][2]*m.mt[2][0] + mt[1][2]*m.mt[2][1] + mt[2][2]*m.mt[2][2]);		
	return *this= temp;
}

Matrix3 & Matrix3::operator*=( const float s )
{
	for(int i=0;i<3;i++)
	{
		for(int j=0;j<3;j++)
		{
			mt[i][j]*=s;
		}
	}
	return *this;
}

Vector3 Matrix3::operator*( const Vector3 &v ) const
{	
	return Vector3(v.x*mt[0][0] + v.y*mt[1][0] + v.z*mt[2][2], 
		v.x*mt[0][1] + v.y*mt[1][1] + v.z*mt[2][1], 
		v.x*mt[0][2] + v.y*mt[1][2] + v.z*mt[2][2]);	
}	

Matrix3 & Matrix3::operator/=( const float s )
{
	for(int i=0;i<3;i++)
	{
		for(int j=0;j<3;j++)
		{
			mt[i][j]/=s;
		}
	}		
	return *this;
}

Matrix3 Matrix3::operator+( const Matrix3 &m ) const
{
	Matrix3 mat;
	for(int i=0;i<3;i++)
	{
		for(int j=0;j<3;j++)
		{
			mat.mt[i][j]=mt[i][j]+m.mt[i][j];
		}
	}
	return mat;
}

Matrix3 & Matrix3::operator+=( const Matrix3 &m )
{
	for(int i=0;i<3;i++)
	{
		for(int j=0;j<3;j++)
		{
			mt[i][j]+=m.mt[i][j];
		}
	}
	return *this;
}

Matrix3 Matrix3::operator-( const Matrix3 &m ) const
{   
	Matrix3 mat;
	for(int i=0;i<3;i++)
	{
		for(int j=0;j<3;j++)
		{
			mat.mt[i][j]=mt[i][j]-m.mt[i][j];
		}
	}
	return mat;
}	

Matrix3 & Matrix3::operator-=( const Matrix3 &m )
{
	for(int i=0;i<3;i++)
	{
		for(int j=0;j<3;j++)
		{
			mt[i][j]-=m.mt[i][j];
		}
	}
	return *this;
}

float Matrix3::determinant() const
{	
	return	mt[0][0]*(mt[1][1]*mt[2][2]-mt[1][2]*mt[2][1]) - 
			mt[1][0]*(mt[0][1]*mt[2][2]-mt[0][2]*mt[2][1]) + 
			mt[2][0]*(mt[0][1]*mt[1][2]-mt[0][2]*mt[1][1]);  
}

Vector3 & Matrix3::multiply_Vector3Matrix3( Vector3 &vecdesc,const Vector3 &vecsrc,const Matrix3 &mat )
{
	vecdesc.x=vecsrc.x*(mat.mt[0][0]+mat.mt[0][1]+mat.mt[0][2]);
	vecdesc.y=vecsrc.y*(mat.mt[1][0]+mat.mt[1][1]+mat.mt[1][2]);
	vecdesc.z=vecsrc.z*(mat.mt[2][0]+mat.mt[2][1]+mat.mt[2][2]);
	return vecdesc;
}
